
#pragma warning (disable : 4251 4996 4819 4244)
#include <new>
#include <stdexcept>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <ctime>
#include "ompl/util/RandomNumbers.h"
#include <boost/shared_array.hpp>
#include <boost/random.hpp>
#include <libplayerc++/playerc++.h>
#include "StateRecorder.h"
#include "Order.h"
#include "Executor.h"
#include "PlayerObject.h"
#include "PlayerObjectManager.h"
#include "pause.h"
#include "PathPlanner.h"
#include "PlayerProxy.h"
#include "FrictionModelImpl.h"
#include "VisualizerImpl.h"

using namespace PlayerCc;
using namespace ompl::base;
using namespace ompl::geometric;


static const double MAX_PLAN_TIME = 0.01;

class Controller {

  ompl::RNG rng;

  // player parameters
  PlayerClient&    robot;
  SimulationProxy sim;
  MapProxy&        map;
  Position2dProxy pp;
  Position2dProxy gocon;
  Graphics2dProxy g;

  // standby pos is robot start
  double standbyX;
  double standbyY;

  // tables
  const std::vector<PlayerObject> & tables;

  // order simulator
  std::auto_ptr<customer::OrderSimulator> ordersim;
  double reachDist;

  // friction model
  boost::shared_ptr<FrictionModel> frictionModel;

public:

  Controller() :
    robot(PlayerProxy::Instance().GetClient()),
    sim(&robot, 0),
    map(PlayerProxy::Instance().GetMap()),
    pp(&robot, 0),
    gocon(&robot, 100),
    g(&robot, 0),
    tables(PlayerObjectManager::Instance().GetStaticObjects()),
    reachDist(0.5),
    frictionModel(new FrictionModelImpl(sim))
  {
    robot.Read();

    PathPlanner::Instance().Init( map, frictionModel, true );

    boost::shared_ptr<Visualizer> visualizer(new VisualizerImpl(g, pp));
    PathPlanner::Instance().setVisualizer(visualizer);

    // standby position is start pos
    standbyX = pp.GetXPos();
    standbyY = pp.GetYPos();
    std::cout << "Standby Pos(" << standbyX << ", " << standbyY << ")" <<std::endl;

    // get tables and init order sim
    ordersim.reset(new customer::OrderSimulator(tables.size()));
  }

private:

  // plan motion from one start to one goal and execute
  void planAndExecute(const std::string & description, double goalX, double goalY) {
    // get current robot positions etc.
    robot.Read();

    // visualize
    std::cout << description.c_str() << " (" << goalX << ", " << goalY << ")" << std::endl;

    bool solved = false;
    PathGeometric& refSolution = PathPlanner::Instance().Plan( solved, pp.GetXPos(), pp.GetYPos(), goalX, goalY, 0.5 );

    // execute
    if (solved) {
       Executor executor(refSolution, robot, pp, gocon);
       executor.execute();
    }

    std::cout << description.c_str() << " (" << goalX << ", " << goalY << ") " << (solved? "done": "failed!") << std::endl;

    if (solved) {
       util::pause(5);
    }
    

  }

  void standby() {
    planAndExecute("Standby", standbyX, standbyY);
  }

  // generate a goal for a particular table, not necessary valid
  std::pair<double,double> generateGoal(const PlayerObject & table) {
    double x, y;

    // generate X
    if (rng.uniformBool()) {
      x = rng.uniformReal( table.PosLowX - reachDist, table.PosLowX );
    } else {
      x = rng.uniformReal( table.PosHighX, table.PosHighX + reachDist );
    }

    // generate Y
    if (rng.uniformBool()) {
        y = rng.uniformReal( table.PosLowY - reachDist, table.PosLowY );
    } else {
        y = rng.uniformReal( table.PosHighY, table.PosHighY + reachDist );
    }

    return std::make_pair(x,y);
  }

  // generates a valid goal position
  std::pair<double,double> generateValidGoal(int tableIndex) {
    const PlayerObject & table = tables[tableIndex];
    for (int i = 0; i < 100; ++i) {
      std::pair<double,double> gg = generateGoal(table);
      // check state valid
      if ( PathPlanner::Instance().IsPosValid( gg.first, gg.second )) {
        return gg;
      }
    }
    throw std::runtime_error("no valid goal position!");
  }

  void deliver(const std::vector<customer::Order> orders) {

    int deliverSize = orders.size();
    int deliverNum = 1;

    std::cout << "Delivering " << deliverSize << " orders" << std::endl;

    for (std::vector<customer::Order>::const_iterator i = orders.begin();
      i != orders.end(); ++i) {

      std::string tableName = tables[i->table].Name;
      std::pair<double,double> goal = generateValidGoal(i->table);

      std::stringstream description;
      description << "Deliver_" << deliverNum++ << "_of_" << deliverSize << "_to_" << tableName;

      planAndExecute(description.str(), goal.first, goal.second);
    }
  }

public:

  // control loop
  void run() {

    for (;;) {
        // generate a list of orders to fulfil
        std::vector<customer::Order> orders = ordersim->getOrders();
        std::cout << "Order size" << orders.size() << std::endl;

      // if no orders then standby, else deliver them
      if (!orders.empty()) {
        deliver(orders);
        standby();
      }
    }

  }
};

int
main(int argc, char *argv[])
{
    std::vector<std::string> vecDefFiles;
    vecDefFiles.push_back( "../worlds/tables.inc" );
    vecDefFiles.push_back( "../worlds/person.inc" );
    PlayerObjectManager::Instance().ParseConfigFile( vecDefFiles, "../worlds/simple.world" );
  try {
    Controller control;
    control.run();
    return 0;
  } catch (std::exception &e) {
    std::cerr << e.what() << std::endl;
    return 1;
  }
}
